#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>

#include "Body.h"

#define EPS 1e-11


std::ostream& operator<<(std::ostream& str, Body& body){
	
	str << " [Id] < " << body.part.id << " > " 
	    << " [Mass] < " << body.part.mass << " > "
	    << " [Position(x,y,z)] < "
	    << body.part.posx << " , " << body.part.posy  << " , " << body.part.posz
            << " > [Velocity (x,y,z)] < "
            << body.getVelX() << " , " << body.getVelY()  << " , " << body.getVelZ() << " > " << std::endl;
	return str;
}	

double drand(int max){
	return ((double)(rand()%(max*10) + 1))/(10.0);
}

/***************************************************************/
/** VECTOR *****************************************************/
/***************************************************************/

//Vector-Constructor: Vector()
Vector::Vector():mX(0),mY(0),mZ(0){
}

//Vector-Constructor: Vector(double x, double y, double z)
Vector::Vector(double x, double y, double z):mX(x),mY(y),mZ(z){
}

//Vector-Constructor: Vector(Particle& part)
Vector::Vector(Particle& part){
	this->mX = part.posx;
	this->mY = part.posy;
	this->mZ = part.posz;
}

//Vector-clear()
//Sets the membervariables to zero
void Vector::clear(){
	this->mX = 0;
	this->mY = 0;
	this->mZ = 0;
}

/***************************************************************/
/** BODY *******************************************************/
/***************************************************************/

//Body-Constructor Body()
Body::Body(){}

//Body-Destructor Body()
Body::~Body(){}

void Body::init(unsigned int id, double dt){
	this->part.id = id;
	
	this->mDt = dt;
	
	//set mass
	srand(id + dt);	
	this->part.mass = drand(100000);

	//set pos
	this->part.posx = drand(10);
	this->part.posy = drand(10);
	this->part.posz = drand(10);
	
	//set accelertion to 0
	this->accelertion.clear();
}

//Body-calcVelocity()
//performs the calculation f1 = sum(mj*dji/dist^3) & accelertion = accelertion + f1*GRAVCONST
//mj - mass of the foreign particle given by the parameter particle
//dij - delta-values -> dx/dy/dz
//dist - distance between the current body and the foreign particle
void Body::calcAcceleration(Particle* particle, int amount){
	int j = 0;
	double ax = 0;
	double ay = 0;
	double az = 0;
	//Force = gamma*mi*(sum(mj*dij/dist^3))
	for(j = 0; j < amount; j++){
		double dist = 0;
		double m = 0;
		double rx = 0;
		double ry = 0;
		double rz = 0;

		//calc distance
		rx = this->part.posx - particle[j].posx;
		ry = this->part.posy - particle[j].posy;
		rz = this->part.posz - particle[j].posz;
	
	/*
		std::cout << std::endl << "rx " << rx << " = " << this->part.posx << " - " << particle[j].posx 
				       << "ry " << ry << " = " << this->part.posy << " - " << particle[j].posy
				       << "rz " << ry << " = " << this->part.posz << " - " << particle[j].posz 
				       << std::endl << std::endl; 
  	*/
		//calc(dist^2+EPS)
		dist = rx*rx + ry*ry + rz*rz + EPS;

		//calc mj/(dist)^(3/2)
		m = particle[j].mass / sqrt(dist*dist*dist);

		ax += rx*m; //if foreign == current obj: m > 0 && rx == 0 => ax = 0;
		ay += ry*m;
		az += rz*m;		 		
	}

	this->accelertion.x() += ax; 
	this->accelertion.y() += ay;
	this->accelertion.z() += az;
}

void Body::calcAcceleration(Body* body, int amount){
	int j = 0;
	
	double ax = 0;
	double ay = 0;
	double az = 0;
	//Force = gamma*mi*(sum(mj*dij/dist^3))
	for(j = 0; j < amount; j++){
		double dist = 0;
		double m = 0;
		double rx = 0;
		double ry = 0;
		double rz = 0;

		//calc distance
		rx = this->part.posx - body[j].part.posx;
		ry = this->part.posy - body[j].part.posy;
		rz = this->part.posz - body[j].part.posz;
		
		//calc(dist^2+EPS)
		dist = rx*rx + ry*ry + rz*rz + EPS;

	/*
		std::cout << std::endl << "rx " << rx << " = " << this->part.posx << " - " << body[j].part.posx
				       << "ry " << ry << " = " << this->part.posy << " - " << body[j].part.posy
				       << "rz " << ry << " = " << this->part.posz << " - " << body[j].part.posz
				       << std::endl << std::endl;
	*/
		//calc mj/(dist)^(3/2)
		m = body[j].part.mass / sqrt(dist*dist*dist);
		
		ax += rx*m; //if foreign == current obj: m > 0 && rx == 0 => ax = 0;
		ay += ry*m;
		az += rz*m;		 		
	}

	this->accelertion.x() += ax; 
	this->accelertion.y() += ay;
	this->accelertion.z() += az;
}

//Body-updatePosition()
void Body::updatePosition(){
	//calculate new positions
	double dt = (-1)*GRAVCONST*mDt*mDt;
	this->part.posx = this->part.posx + this->accelertion.x() * dt;
	this->part.posy = this->part.posy + this->accelertion.y() * dt;
	this->part.posz = this->part.posz + this->accelertion.z() * dt;
}

void
Body::setMass(double mass){
	this->part.mass = mass;
}

void
Body::setDt(double dt){
	this->mDt = dt;
}

void
Body::setPos(double x, double y, double z){
	this->part.posx = x;
	this->part.posy = y;
	this->part.posz = z;
}
